#ifndef __SERIAL_C__HPP__
#define __SERIAL_C__HPP__
#include <iostream>
#include <cstring>
#include <queue>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <stdexcept>
#include <vector>
#include <functional>
#include <chrono>
#include <fstream>
#include <atomic>
#pragma pack(push,1)
/**
 * @brief PID结构体
 * 
 */
typedef struct
{
    float kp;
    float ki;
    float kd;
    float lim_sum_error;
    float lim_output;
}user_pid_t;
/**
 * @brief 发送结构体
 * @note pitch和yaw共用一个结构体
 */
class send_data1_t
{
    public:
    send_data1_t():x_3d(0),y_3d(0),door_status(0),data_valid(0)
    {}
    
    float center_x;
    float center_y;
    float x_3d;
    float y_3d;
    uint8_t data_valid;
    uint8_t door_status;
};
class send_data2_t
{
    public:
    send_data2_t():roll_pos({1,0,0,30,10}),roll_spd({1,0,0,30,10}),
    pitch_KA({-10,0,0,30,100}),
    pitch_PID_intergral({0,10,0,30,100}),
    pitch_PID_KR({-2,0,0,30,20})
    {}
    user_pid_t roll_pos;
    user_pid_t roll_spd;
    user_pid_t pitch_KA;
    user_pid_t pitch_PID_intergral;
    user_pid_t pitch_PID_KR;
};
#pragma pack(pop)
extern send_data1_t user_send_data1;
extern send_data2_t user_send_data2;
/**
 * @brief 接收数据结构体
 * 
 */
class Receive_Data_C {
    public:
    float dart_status;
    float pitch_angle;
    float yaw_angle;
    float roll_angle;
    float temperature;
    float delta_x;
    float delta_y;
    float delta_z;
    float a_x;
    float a_y;
    float a_z;
    float omega_x;
    float omega_y;
    float omega_z;
    float vdd_voltage;
    float vbx;
    float vby;
    float vbz;
    float alpha;
    float beta;

    Receive_Data_C(std::ofstream& _file_output) : dart_status(-1), pitch_angle(0), yaw_angle(0), roll_angle(0), temperature(0),
    delta_x(0), delta_y(0), delta_z(0), a_x(0), a_y(0), a_z(0),
    omega_x(0), omega_y(0), omega_z(0), vdd_voltage(0),
    vbx(0), vby(0), vbz(0), alpha(0), beta(0),
    file_output(_file_output) {
        //file_output.open("./output_data0.txt", std::ios::out);
        if(!file_output.is_open()) {
            std::cerr << "open file failed" << std::endl;
        }
        start_time = std::chrono::steady_clock::now();
    }
    ~Receive_Data_C() {   
        if(file_output.is_open()) {
            file_output.close();
        }
        else std::cout << "~function file not open" << std::endl;
        std::cout << "exit" << std::endl;
    }
    void my_print();
    void data_arrange(const char* data);
    void Data_Storage();
    private:
    std::ofstream& file_output;
    std::chrono::steady_clock::time_point start_time;
    std::chrono::steady_clock::time_point now_time;
    std::chrono::duration<float> duration;
};

#define BUFFER_SIZE 256
#define MYPACK_FRAMEHEADER_SOF1 0xA5    
#define MYPACK_FRAMEHEADER_SOF2 0xA6

/**
 * @brief 接收buffer
 * 
 */
class RXBuffer {
    public:
        size_t length;
        std::vector<uint8_t> buffer;
        std::vector<uint8_t> all_data;
        uint16_t CRC_data;
        bool CRC_success;
        size_t bag_count;

        RXBuffer() : length(0), buffer(BUFFER_SIZE), all_data(BUFFER_SIZE + 5), CRC_data(0), CRC_success(false), bag_count(0) {}
};
/**
 * @brief 解包枚举
 * 
 */
enum PACKRX_STATUS_ENUM {
    MyPack_Frame_SOF = 0,
    MyPack_Frame_DataLength,
    MyPack_Frame_DATA,
    MyPack_Frame_CRC16
};
/**
 * @brief 状态机解包函数
 * 
 */
class Unpacker {
    public:

        PACKRX_STATUS_ENUM PackRX_Status;
        size_t SOF_count;
        size_t DataLenth_count;
        size_t CRC16_count;
        RXBuffer RX_Buffer;

        Unpacker() : PackRX_Status(MyPack_Frame_SOF), SOF_count(0), DataLenth_count(0), CRC16_count(0) {}
};

class SerialProtocolHandler {
    public:
    SerialProtocolHandler(const std::string& port, int baudrate, int data_bits, int stop_bits, char parity,
        std::function<void(const std::vector<uint8_t>&)> callback);

    ~SerialProtocolHandler();

    void start();
    void stop();
    void start_cv();
    void send(const std::vector<uint8_t>& data);
    void send(const void* message,const uint8_t message_size);
    void checkbuffersize()
    {
        int bytes;
        ioctl(this->fd, FIONREAD, &bytes);
        std::cout << "Bytes in buffer: " << bytes << std::endl;
    }
    void setbuffersize(int size)
    {
        ioctl(this->fd, TIOCSWINSZ, size);
    }

    private:
    void configure_serial_port();
    void _receive_thread();
    void _unpack_byte(uint8_t byte);
    bool _verify_crc(const uint8_t* data, size_t length);
    uint16_t _get_crc16(const uint8_t* data, size_t length, uint16_t crc_init);
    void _process_thread();
    void _process_thread_cv();
    void _append_CRC16_Check_Sum(uint8_t * pchMessage,uint32_t dwLength);

    
    static int receive_thread_ms;
    static int process_thread_ms;
    std::string port;
    int baudrate;
    int data_bits;
    int stop_bits;
    char parity;
    std::function<void(const std::vector<uint8_t>&)> callback;
    int fd;
    std::atomic<bool> _running;
    uint8_t data[BUFFER_SIZE];
    std::thread receiver_thread;
    std::thread processor_thread;
    std::mutex mtx;
    std::condition_variable cv;
    std::queue<std::vector<uint8_t>> receive_queue;
    Unpacker unpacker;
};
#endif // __SERIAL_C__HPP__